/*
 * This file is part of the Chumby Computer Vision Project: www.EMGRobotics.com
 *
 * Copyright (c) 2010 individual Chumby Computer Vision contributors. 
 * See end of header for contributor list.
 *
 * This code is licensed to you under the terms of the Apache License, version
 * 2.0, or, at your option, the terms of the GNU General Public License,
 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
 * or the following URLs:
 * http://www.apache.org/licenses/LICENSE-2.0
 * http://www.gnu.org/licenses/gpl-2.0.txt
 *
 * If you redistribute this file in source form, modified or unmodified, you
 * may:
 *   1) Leave this header intact and distribute it under the same terms,
 *      accompanying it with the APACHE20 and GPL20 files, or
 *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
 *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
 *
 * In all cases you must keep this copyright notice intact, along with the
 * list of contributors at the end of this header.
 *
 * Binary distributions must follow the binary distribution requirements of
 * either License.
 *
 * Contributors: 
 *			Eric Gregori ( www.EMGRobotics.com )
 */
//
//**********************************************************************************
//
//  ChumbyCV Algorithm - Motion Detection
//
//  Compile this file, with a file containing your algorithm ( see example algorithm files )
//  gcc -L/usr/lib  -lXt -lX11 -lXaw ChumbyCV_X11.c ChumbyCV_MotionDetect.c -o ChumbyCV_X11
//  Replace ChumbyCV_Motion.c  with your algorithm file.
//
//  Don't forget to chmod 777 ChumbyCV_X11 after building 
//
//
//  Command Line Parameters:
//
//
//**********************************************************************************
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>

//**********************************************************************************
// CallBack function
//**********************************************************************************
extern inline void SetPixel( uint32_t index, unsigned char r, unsigned char g, unsigned char b );

//**********************************************************************************
// Forward Declarations
//**********************************************************************************
int ChumbyCV_MotionDetect_Init( void );

//**********************************************************************************
// Globals
//**********************************************************************************
uint32_t	G_Width, G_Height;
uint32_t	G_NumOfPixels;
unsigned char	G_ThresHold;
unsigned char	G_AutoThres;
unsigned char  *G_PreviousFrame;
unsigned char	G_Size;
uint32_t       *G_IntegralSpace;

//**********************************************************************************
// Command Line Parameters:
//	argv[1] - video Device ( /dev/video0 )
//	argv[x] - Options
//			-txxx	Set Threshold ( default operation auto adjusts threshold	
//				Where xxx = decimal # from  1-254
//
// Return 0 for no error, !0 causes exit(1)
//**********************************************************************************
int ChumbyCV_Parameters( int argc, char *argv[], int width, int height )
{
	int	i, bad;

	G_Width  	= width;
	G_Height 	= height;
	G_NumOfPixels	= G_Width*G_Height;
	G_AutoThres	= 1;
	G_ThresHold	= 0;
	G_Size		= 10;

	fprintf( stderr, "\nChumbyCV Motion Detection Algorithm\n" );
	if( argc > 2 )
	{
		for( i=2, bad=1; i<argc; ++i )
		{
			if( argv[i][0] != '-' )
				break;
		
			switch( argv[i][1] )
			{
				case 't':
					G_ThresHold = atoi( &argv[i][2] );
					G_AutoThres	= 0;
					if( G_AutoThres < 255 ) bad = 0;
					break;

				case 's':
					G_Size = atoi( &argv[i][2] );
					bad    = 0;
					break;

				default:
					break;
			}		
		}

		if( bad )
		{
			fprintf( stderr, "\nUsage: %s V4L2_device [-txxx]\n", argv[0] );
			fprintf( stderr, "-txxx Set threshold to decimal xxx ( 0-255 )\n" );
			fprintf( stderr, "default is auto threshold adjust\n" );
			return 1;
		}

	}


	if( ChumbyCV_MotionDetect_Init() > 0 )
		return 1;

	return 0; 
}

void ChumbyCV_ShutDown( void )
{
	free( G_PreviousFrame );
	free( G_IntegralSpace );
}

//**********************************************************************************
// Init Algorithm
//**********************************************************************************
int ChumbyCV_MotionDetect_Init( void )
{
	// There are 4 bytes per 2 pixels in YUVY
	G_PreviousFrame = (unsigned char *)malloc( G_NumOfPixels*2 );
	if( G_PreviousFrame == NULL )	
		return 1;
	G_IntegralSpace = (uint32_t *)malloc( G_NumOfPixels*sizeof(uint32_t) );
	if( G_IntegralSpace == NULL )	
		return 1;
	
	return 0;
}


//**********************************************************************************
//**********************************************************************************
static inline void DisplayCrossHairs( uint32_t pos )
{
	uint32_t	i,k;

	// Display crosshairs at xpos, ypos
	for(i=(pos%G_Width); i<G_NumOfPixels; i+=G_Width )
	{
		SetPixel(i,0,255,0);
	}

	k = pos-(pos%G_Width);
	for( i=k; i<(k+G_Width); ++i )
	{
		SetPixel(i,0,255,0);
	}
}


//**********************************************************************************
//**********************************************************************************
static inline unsigned char DiffPixel( unsigned char p1, unsigned char p2 )
{
	if( p1 > p2 ) 
		return(p1-p2);
	else
		return(p2-p1);
}

//**********************************************************************************
// ChumbyCV_Algorithm - Motion Detection using frame subtraction
//
// This function is called by the ChumbyCV framework.  The char pointer CameraData
// points to a byte array of size (G_NumOfPixels*2).
// The data format is YUYV => 4 bytes = 2 pixels ( Y=pixel, U=color, V=color ) 
//
// Send result to stdio using format: <x,%x>
// Where "x" is the field name, and the value is after the ',' in hex
//**********************************************************************************
int ChumbyCV_Algorithm( unsigned char* CameraData )
{
	uint32_t CameraIndex;
	uint32_t DisplayIndex;
	uint32_t DiffCount;
	unsigned char y1, y2, u, v, oy1, oy2, ou, ov, temp;
	uint32_t start, count, area1, area2, maxarea, pos;

	DiffCount 	= 0;
	CameraIndex 	= 0;
	memset( G_IntegralSpace, 0, G_NumOfPixels*sizeof(uint32_t) );
	for(DisplayIndex=0; DisplayIndex<G_NumOfPixels; ++DisplayIndex )// DisplayIndex -> used by SetPixel
	{
		// YUYV - 2 pixels for 1u, 1v
		y1 	= CameraData[CameraIndex];			// Current frame from camera
		oy1 	= G_PreviousFrame[CameraIndex++];		// Previous frame from camera
		u  	= CameraData[CameraIndex];
		ou	= G_PreviousFrame[CameraIndex++];
		y2 	= CameraData[CameraIndex];
		oy2	= G_PreviousFrame[CameraIndex++];
		v  	= CameraData[CameraIndex];		
		ov	= G_PreviousFrame[CameraIndex++];

		// pixel 1
		if( DiffPixel(y1,oy1) > G_ThresHold )
		{
			// pixel Difference detected
			temp = 1;					
			SetPixel(DisplayIndex,255,255,255);	// DisplayIndex=0 to G_NumOfPixels
			++DiffCount;
		}
		else
		{
			// No pixel difference detected
			SetPixel(DisplayIndex,0,0,0);		// DisplayIndex=0 to G_NumOfPixels
			temp = 0;
		}
	
		// Update Integral Space
		if( (DisplayIndex>G_Width)&&(DisplayIndex%G_Width) )
		{
			// http://en.wikipedia.org/wiki/Summed_area_table
			G_IntegralSpace[DisplayIndex] = temp +			//x,y
				G_IntegralSpace[DisplayIndex-1]+		//x-1,y
				G_IntegralSpace[DisplayIndex-G_Width]-		//x,y-1 
				G_IntegralSpace[DisplayIndex-G_Width-1];	//x-1,y-1
		}
	
		DisplayIndex++;
		// pixel 2
		if( DiffPixel(y2,oy2) > G_ThresHold )
		{
			// pixel Difference detected
			temp = 1;					
			SetPixel(DisplayIndex,255,255,255);	// DisplayIndex=0 to G_NumOfPixels
			++DiffCount;
		}
		else
		{
			// No pixel difference detected
			SetPixel(DisplayIndex,0,0,0);		// DisplayIndex=0 to G_NumOfPixels
			temp = 0;
		}
	
		// Update Integral space
		if( (DisplayIndex>G_Width)&&(DisplayIndex%G_Width) )
		{
			// http://en.wikipedia.org/wiki/Summed_area_table
			G_IntegralSpace[DisplayIndex] = temp +
				G_IntegralSpace[DisplayIndex-1]+
				G_IntegralSpace[DisplayIndex-G_Width]-
				G_IntegralSpace[DisplayIndex-G_Width-1];
		}

	} // for(DisplayIndex=0;

#if 0 // Display Integral Parameter Space for testing
	for(count=0; count<G_NumOfPixels; ++count )
	{
		area1 = G_IntegralSpace[count];
		if( area1 > 255 ) 
			y1 = 255;
		else
			y1 = area1;
		SetPixel(count,area1/0x100,y1,0);
	}

#endif

	// Look for max window of size ( G_Size ) in Integral Space
	start = (G_Size+(G_Size*G_Width));
	maxarea = G_Size*G_Size/10;
	pos     = 0;
	for(count=start; count<G_NumOfPixels; ++count )
	{
		if( (count%G_Width)<G_Size ) continue;
		// http://en.wikipedia.org/wiki/Summed_area_table
		area1 = (G_IntegralSpace[count] + 			// C
			G_IntegralSpace[count-start]); 			// A
		area2 = G_IntegralSpace[count-G_Size] +			// D
			G_IntegralSpace[count-(G_Size*G_Width)];	// B
		if( (area1-area2) >= maxarea )
		{
			maxarea = (area1-area2);
			pos = count;
			SetPixel(count,0,0,255);
		}
	}

	if( pos )
	{
		pos -= (G_Size/2);
		pos -= (G_Width*G_Size/2);
	}

	printf( "<x,%x><y,%x>\n", pos%G_Width, pos/G_Width ); 

	// Display position
	DisplayCrossHairs( pos );

	// Copy Current FrameBuffer to PreviousFrame
	memcpy( G_PreviousFrame, CameraData, (G_NumOfPixels*2) );
		
	if( G_AutoThres )
	{
		if( DiffCount == 0 )
			G_AutoThres = 0;

		++G_ThresHold;
		for(count=0; count<G_NumOfPixels; ++count )
		{
			if( (count%G_Width) < G_ThresHold )
				SetPixel(count,255,0,0);
		}
	}

	// end of Algorithm

// Simple sample code
// Display image from camera on screen
//
#if 0 // Simply display image on screen
	for(i=0, index=0; i<G_NumOfPixels; i++ )
	{
		// RGB24 output display - 0x00rrbbgg
		temp = CameraData[index+=2];
		SetPixel(i,temp,temp,temp);
	}
#endif
	return 0;
}

